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Abstract  -  In  [Simon  and  Chia,  2002],  an  analytic 
method  was  developed  to  incorporate  linear  state 
equality  constraints  into  the  Kalman  filter.  When  the 
state  constraint  is  nonlinear,  linearization  was  employed 
to  obtain  an  approximately  linear  constraint  around  the 
current  state  estimate.  This  linearized  constrained 
Kalman  filter  is  subject  to  approximation  errors  and 
may  suffer  from  a  lack  of  convergence.  In  this  paper, 
we  present  a  method  that  allows  exact  use  of  second- 
order  nonlinear  state  constraints.  It  is  based  on  a 
computational  algorithm  that  iteratively  finds  the 
Lagrangian  multiplier  for  the  nonlinear  constraints. 
The  method  therefore  provides  better  approximation 
when  higher  order  nonlinearities  are  encountered. 
Computer  simulation  results  are  presented  to  illustrate 
the  algorithm. 

Keywords:  Kalman  filtering,  nonlinear  state  constraints, 
Lagrangian  multiplier,  iterative  solution,  tracking. 


1  Introduction 

In  a  recent  paper  [Simon  and  Chia,  2002],  a  rigorous 
analytic  method  was  set  forth  to  incorporate  linear  state 
equality  constraints  into  the  Kalman  filtering  process. 
Such  constraints  (e.g.,  known  model  and  signal 
information)  are  often  ignored  or  dealt  with  heuristically. 
The  resulting  estimates,  even  obtained  with  the  Kalman 
filter,  cannot  be  optimal  because  they  do  not  take 
advantage  of  this  additional  information  about  state 
constraints. 

One  example  that  benefits  from  state  constraints  is  the 
ground  target  tracking.  When  a  vehicle  travels  off-road  or 
on  an  unknown  road,  the  state  estimation  problem  is 
unconstrained.  However,  when  the  vehicle  is  traveling  on 
a  known  road,  be  it  straight  or  curved,  the  state  estimation 
problem  can  be  cast  as  constrained  with  the  road  network 
information  available  from,  say,  digital  terrain  maps 
[Yang,  Bakich,  and  Blasch,  2005]. 

To  make  use  of  state  constraints,  previous  attempts  range 
from  reducing  the  system  model  parameterization  to 
treating  state  constraints  as  perfect  measurements.  The 
constrained  Kalman  filter  proposed  in  [Simon  and  Chia, 
2002]  consists  of  first  obtaining  an  unconstrained  Kalman 
filter  solution  and  then  projecting  the  unconstrained  state 
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estimate  onto  the  constrained  surface.  Although  the  main 
results  are  restricted  to  linear  systems  and  linear  state 
equality  constraints,  the  authors  outlined  steps  to  extend  it 
to  inequality  constraints,  nonlinear  dynamics  systems,  and 
nonlinear  state  constraints. 

According  to  [Simon  and  Chia,  2002],  the  inequality 
constraints  can  be  checked  at  each  time  step  of  the  filter. 
If  the  inequality  constraints  are  satisfied  at  a  given  time 
step,  no  action  is  taken  since  the  inequality  constrained 
problem  is  solved.  If  the  inequality  constraints  are  not 
satisfied  at  a  given  time  step,  then  the  constrained  solution 
is  applied  to  enforce  the  constraints.  Furthermore,  to  apply 
the  constrained  Kalman  filter  to  nonlinear  systems  and 
nonlinear  state  constraints,  it  is  suggested  in  [Simon  and 
Chia,  2002]  to  linearize  both  the  system  and  constraint 
equations  about  the  current  state  estimate.  The  former  is 
equivalent  to  the  use  of  an  extended  Kalman  filter  (EKF). 

However,  the  projection  of  the  unconstrained  state 
estimate  onto  a  linearized  state  constraint  is  subject  to 
constraint  approximation  errors,  which  is  a  function  of  the 
nonlinearity  and  more  importantly  the  point  around  which 
the  linearization  takes  place.  This  may  result  in 
convergence  problems.  It  was  suggested  in  [Simon  and 
Chia,  2002]  to  take  extra  measures  to  guarantee 
convergence  in  the  presence  of  nonlinear  constraints. 

There  are  a  host  of  constrained  nonlinear  optimization 
techniques  [Luenberger,  1989].  Primal  methods  search 
through  the  feasible  region  determined  by  the  constraints. 
Penalty  and  barrier  methods  approximate  constrained 
optimization  problems  by  unconstrained  problems  through 
modifying  the  objective  function  (e.g.,  add  a  term  for 
higher  price  if  a  constraint  is  violated).  Instead  of  the 
original  constrained  problem,  dual  methods  attempt  to 
solve  an  alternate  problem  (the  dual  problem)  whose 
unknowns  are  the  Lagrangian  multipliers  of  the  first 
problem.  Cutting  plane  algorithms  work  on  a  series  of 
ever-improving  approximating  linear  programs  whose 
solutions  converge  to  that  of  the  original  problem. 
Lagrangian  relaxation  methods  are  widely  used  in  discrete 
constrained  optimization  problems. 

In  this  paper,  we  present  a  method  that  allows  for  the  use 
of  second-order  nonlinear  state  constraints  exactly.  The 
method  can  provide  better  approximation  to  higher  order 
nonlinearities.  The  new  method  is  based  on  a 
computational  algorithm  that  iteratively  finds  the 
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Lagrangian  multiplier.  Considering  only  second-order 
constraints  in  this  paper  is  a  tradeoff  between  reducing 
approximation  errors  to  higher-order  nonlinearities  and 
keeping  the  problem  computationally  tractable. 

In  general,  the  solution  to  a  fdtering  problem  is  an  a 
posteriori  probability  density  function  (pdf)  and  in  the 
linear  Gaussian  case,  it  is  normally  distributed  with  a 
mean  vector  and  a  covariance  matrix.  At  a  first  glance, 
such  a  solution  can  never  be  the  solution  to  a  filtering 
problem  with  a  hard  constraint  simply  because  a  pdf 
compliant  with  a  hard  constraint  cannot  have  support  on 
the  whole  state  space,  as  pointed  in  [Anonym,  2006].  In 
this  paper  and  in  [Simon  and  Chia,  2002],  the  constrained 
projection  actually  maps  the  whole  state  space  onto  the 
constraints,  producing  a  constrained  pdf  with  its  support 
on  the  constraint  surface.  Although  not  explicitly  in  this 
paper,  the  constrained  pdf  can  be  derived  for  the  second- 
order  constraints  in  this  paper,  from  which  the  estimate 
statistics  can  in  turn  be  determined  albeit  approximately. 

The  paper  is  organized  as  follows.  Section  2  presents  a 
brief  summary  of  linearly  constrained  state  estimation  and 
its  problems  encountered  when  the  first-order 
linearization  is  used  to  extend  to  nonlinear  cases.  Section 
3  details  the  computational  algorithm  to  solve  the  second- 
order  constrained  least-squared  optimization  problem.  In 
Section  4,  computer  simulation  results  are  presented  to 
illustrate  the  algorithm.  Finally,  Section  5  provides 
concluding  remarks  and  suggestions  for  future  work. 


2  Linear  Constrained  State  Estimation 

In  this  section,  we  first  summarize  the  results  for  linearly 
constrained  state  estimation  [Simon  and  Chia,  2002]  and 
then  point  out  the  problems  it  may  face  when  the 
linearization  is  used  to  extend  it  to  nonlinear  constraints. 

Consider  a  linear  time-invariant  discrete -time  dynamic 
system  together  with  its  measurement  as 

(la) 

l,=Cx,+v,  (lb) 

where  the  underscore  indicates  a  vector  quantity,  the 
subscript  k  is  the  time  index,  x  is  the  state  vector,  m  is  a 
known  input,  y  is  the  measurement,  and  w  and  y  are  state 
and  measurement  noise  processes,  respectively.  It  is 
implied  that  all  vectors  and  matrices  have  compatible 
dimensions,  which  are  omitted  for  simplicity. 

The  goal  is  to  find  an  estimate  denoted  by  Xj  of  given 

the  measurements  up  to  time  k  denoted  by  Yt  =  {y_o,  ■■■, 
yjJ.  Under  the  assumptions  that  the  state  and  measurement 
noises  are  uncorrelated  zero-mean  white  Gaussian  with  w 
~  yV(0,  Q}  and  y  ~  YA/(0,  R}  where  Q  and  R  are  positive 
semi-defmite  covariance  matrices,  the  Kalman  fdter 
provides  an  optimal  estimator  in  the  form  of 
[Anderson  and  Moore,  1979].  Starting 


from  an  initial  estimate  =  E{Xf,}  and  its  estimation 
error  covariance  matrix  /])  =  .E{(Xo  -  Xo)(x„  - 

where  the  superscript  T  stands  for  matrix  transpose,  the 
Kalman  fdter  equations  specify  the  propagation  of  and 

Pk  over  time  and  the  update  off^  and  P*  by  measurement 
y>as 


Vtu  =  Ax,  -I-  Bu,  +  K, (y ^  -Cx,)  (2a) 

K,=AP,C^{CP,C^+R)-'  (2b) 

P,,,={AP,-K,CP,)A^  +Q  (2c) 

Now  in  addition  to  the  dynamic  system  of  Eq.  (1),  we  are 
given  the  linear  state  constraint 

Dx,  =d_  (3) 


where  Z)  is  a  known  constant  matrix  of  full  rank,  ^  is  a 
known  vector,  and  the  number  of  rows  in  D  is  the  number 
of  constraints,  which  is  assumed  to  be  less  than  the 
number  of  states.  If  Z)  is  a  square  matrix,  the  state  is  fully 
constrained  and  can  thus  be  solved  by  inverting  Eq.  (3). 
Although  no  time  index  is  given  to  D  and  d  in  Eq.  (3),  it  is 
implied  that  they  can  be  time-dependent,  leading  to 
piecewise  linear  constraints. 

The  constrained  Kalman  fdter  according  to  [Simon  and 
Chia,  2002]  is  constructed  by  directly  projecting  the 
unconstrained  state  estimate  x,  onto  the  constrained 

surface  S  =  {x:  Dx  =  d}.  It  is  formulated  as  the  solution  to 
the  problem: 

X  =  argmin(x-x)^fK(x-x)  (4) 

where  IF  is  a  symmetric  positive  definite  weighting 
matrix.  To  solve  this  problem,  we  form  the  Lagrangian 

/(x,T)  =  (x-x)^lF(x-x)-i-2T^(Z)x-(7)  (5) 

The  first  order  conditions  necessary  for  a  minimum  are 
given  by 


—  =  0^fV(x-x)  +  D^A  =  0  (6a) 

dx 

—  =  Q^Dx-d=Q  (6b) 

dk  -  - 

This  gives  the  solution 

k  =  (DW-'D^y'(Dx-d)  (7a) 

x=x-W-'D^iDW-'D^)-\Dx-d)  (7b) 


The  above  derivation  does  not  depend  on  the  conditional 
Gaussian  nature  of  the  unconstrained  estimate  x-  It  was 
shown  in  [Simon  and  Chia,  2002]  that  when  W  =  I,  the 
solution  in  Eq.  (7)  is  the  same  as  what  is  obtained  by  the 
mean  square  method,  which  attempts  to  minimize  the 
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conditional  mean  square  error  subject  to  the  state 
constraints,  that  is, 

min^jlx-ll^  I  Y}  such  thatDx  =  d  (8) 

where  ||.||  denotes  the  vector  two-norm.  Furthermore, 

II  II2 

when  W  =  P'’ ,  i.e.,  the  inverse  of  the  unconstrained  state 
estimation  error  covariance,  the  solution  in  Eq.  (7) 
reduces  to  the  result  given  by  the  maximum  conditional 
probability  method 

max  In  Prob{x  |  Y}  such  that  Dx^d  (9) 

Several  interesting  statistical  properties  of  the  constrained 
Kalman  filter  are  presented  in  [Simon  and  Chia,  2002]. 
This  includes  the  fact  that  the  constrained  state  estimate  as 
given  by  Eq.  (7)  is  an  unbiased  state  estimate  for  the 
system  in  Eq.  (1)  subject  to  the  constraint  in  Eq.  (3)  for  a 
known  symmetric  positive  definite  weighting  matrix  W. 
Furthermore  when  W  =  P'\  the  constrained  state  estimate 
has  a  smaller  error  covariance  than  that  of  the 
unconstrained  state  estimate,  and  it  is  actually  the  smallest 
for  all  constrained  Kalman  fdters  of  this  type.  Similar 
results  hold  in  terms  of  the  trace  of  the  estimation  error 
covariance  matrix  when  W  ^  I. 

In  practical  applications,  however,  nonlinear  state 
constraints  are  likely  to  emerge.  Consider  the  nonlinear 
state  constraint  of  the  form 

g(^  =  d  (10) 

We  can  expand  the  nonlinear  state  constraints  about  a 
constrained  state  estimate  x  and  for  the  i*  row  of  Eq. 
(10),  we  have 

gi(.x)-d.  =  gi{xyrg'{xY{x-x)  + 

Y^ix-xfg"ix)ix-x)  +  --  --di  =0  (11) 


the  approximate  linear  state  constraint  produces  the 
current  constrained  state  estimate x*,  which  is  however 
subject  to  the  constraint  approximation  error.  Clearly,  the 
further  away  is  x“  from  x,  the  larger  is  the  approximation- 
introduced  error.  More  critically,  such  an  approximately 
linear  constrained  estimate  may  not  satisfy  the  original 
nonlinear  constraint  specified  in  Eq.  (10).  It  is  therefore 
desired  to  reduce  this  approximation-introduced  error  by 
including  higher-order  terms  while  keeping  the  problem 
computationally  tractable.  One  possible  approach  is 
presented  in  the  next  section. 


X  True  State  x  Previous  Constrained  State  Estimate 

X  Unconstrained  State  Estimate  x  Ciurent  Constrained  Sate  Estimate 


Figure  1  -  Errors  in  Linear  Approximation  of  Nonlinear 
State  Constraints 


3  Nonlinear  Constrained  Estimation 

In  this  section,  we  consider  a  second-order  state  constraint 
function,  which  can  be  viewed  as  a  second-order 
approximation  to  an  arbitrary  nonlinearity,  as 


=  ^  M  x  + x  + ^  m  + Mg  =  0  (13) 


where  the  superscripts  '  and  "  denote  the  first  and  second 
partial  derivatives. 

Keeping  only  the  first-order  terms  as  suggested  in  [Simon 
and  Chia,  2002],  some  rearrangement  leads  to 

g'(x)^x»d-g(x>f  (12) 


As  an  example,  a  general  equation  of  the  second  degree  in 
two  variables  ^  and  rj  is  written  as 

f{4,  Tj)  =  +  Ib^T]  +  cif  +ld^  +  leri  +  f 

a  b  d  ^ 

=  \^  t]  \\  b  c  e  7=0 

d  e  f  I 


where  g(x)  =  d  =  [...di...f,  and  g'fx)  = 

[■■■gi'(x)---J ■  An  approximate  linear  constraint  is  therefore 
formed  by  replacing  D  and  d  in  Eq.  (3)  with  g'(x)  and 
d-g(x)+  g'(x)^x,  respectively. 

Figure  1  illustrates  this  linearization  process,  which 
identifies  possible  errors  associated  with  linear 
approximation  of  a  nonlinear  state  constraint.  As  shown, 
the  previous  constrained  state  estimate  x~  lies  somewhere 
on  the  constrained  surface  but  is  away  from  the  true  state. 
The  projection  of  the  unconstrained  state  estimate  x  onto 


which  may  represent  a  road  segment  in  a  digital  terrain 
map. 

Following  the  constrained  Kalman  filtering  of  [Simon  and 
Chia,  2002],  we  can  formulate  the  projection  of  an 
unconstrained  state  estimation  onto  a  nonlinear  constraint 
surface  as  the  constrained  least-square  optimization 
problem 

X  =  argmin(z-/7x)^(z-/7x)  (15) 

X 

subject  to  f(x)  =  0 
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If  we  let  W  =  H^H  and  z  =  Hx ,  the  formulation  in  Eq. 
(15)  beeomes  the  same  as  in  Eq.  (4).  In  a  sense,  Eq.  (15) 
is  a  more  general  formulation  beeause  it  ean  also  be 
interpreted  as  a  nonlinear  eonsbained  measurement 
update  or  a  projeetion  in  the  predieted  measurement 
domain. 


^{l  +  Aaff 

(23a) 

m'' x  =  !{l +  A-Z^I.)-'eiA)  =  Y, 

i  \  +  Aa- 

(23b) 

x^  m  =  e{AY{I  +  AY.''Y.y't  =  Y'^‘^^^‘\ 

-  -  -  -  Y\  +  Aa) 

(23e) 

Constmet  the  Lagrangian  with  the  Lagrangian  multiplier  A, 
as 

J(^  A)  =  (z-H^^(z-H^  +  Af(^  (17) 

Taking  the  partial  derivatives  of  /(x,  A)  with  respeet  to  x 
and  A,  respeetively,  setting  them  to  zero  leads  to  the 
neeessary  eonditions: 

-H^z  +  Am  +  (H^H+AM)x  =  0  (18a) 

x^Mx  +  m^x  +  +  mg  ^  0  (18b) 


Bringing  these  terms  into  the  eonstrained  equation  in  Eq. 
(18b)  gives  rise  to  the  eonstraint  equation,  now  expressed 
in  terms  of  the  unknown  Lagrangian  multiplier  A,  as 


f(A)  =  (^H-  Am^)  (H^H+AM)-^(H\  -  Am) 

+  m^ (H^H+  A  M)'^(H^z  —A jn) 

+  (zjH-  Am!:)  (H^H+AM)-‘m  +  mg 
=  e(A)!l+Al!l))-^l!i:(I+Al!l))-'e(A) 

+  t!(I+AI^Z)-‘e(A)  +  e(A)!l+ AAfAJ)-!_  +  mg 


E 


(l  +  Aaf! 


-lY- 

.1 


e,.(/l)f 


+  Aaf 


-  +  m„ 


(24) 


Assume  that  the  inverse  matrix  of  H^H+AM  exists.  Then, 
X  ean  be  solved  from  Eq.  (18a),  giving  the  eonstrained 
solution  in  terms  of  the  unknown  X  as 

X  =  (H^H+AM)-'(H\  -  Am)  (19) 


As  a  nonlinear  equation  in  A,  it  is  diffieult  to  find  a 
elosed-form  solution  in  general.  Numerieal  root-finding 
algorithms  may  be  used  instead.  For  example,  the 
Newton’s  method  is  used  below.  Denote  the  derivative  of 
f(A)  with  respeet  to  A  as 


whieh  reduees  to  the  uneonstrained  least-squares  solution 
when  A^  0. 

Assume  that  the  matrix  M  admits  the  faetorization  M  = 
L^L  and  apply  the  Cholesky  faetorization  to  IF  =  IrfH  as 


/(^)  =  2X 


eX^!0  +  Aaf)af-ef{Z)a: 


■21 


{\  +  Aay 

e,t,{l  +  Aaf)-eXA)t,af 

{l  +  Aaff 


(25a) 


W{=  H^H)  =  G^G 


where 


where  G  is  an  upper  right  diagonal  matrix.  We  then 
perform  a  singular  value  deeomposition  (SVD)  of  the 
matrix  LG'^  [Moon  and  Stirling,  2000]  as 

LG'  =  UZV''  (21) 

where  U  and  V  are  orthonormal  matriees  and  2"  is  a 
diagonal  matrix  with  its  diagonal  elements  denoted  by  a,. 
For  a  square  matrix,  this  beeomes  the  eigenvalue 
deeomposition. 

Introduee  two  new  veetors 

e(A)  =  f...  ei(A),  ...]  ^  =  V'^(G!-'(H\  -  Am)  (22a) 

t  =  /. . .  t,  . .  .7  ^  =  V^G^-'m  (22b) 

With  these  faetorizations  and  new  matrix  and  veetor 
notations,  the  eonstrained  solution  in  Eq.  (19)  ean  be 
simplified  as 

x=G-'V(l  +  A'Z^Y)-'e(A)  (23) 

The  first  and  seeond  order  terms  in  x  in  Eq.  (18b)  ean  be 
expressed  in  A  as: 

xMx  =  eiAfil  +  +  AYl.y'eiA) 


e  -[...  e.  =  -V!G!-'m  (25b) 


Then  the  iterative  solution  for  X  is  given  by 

,  starting  with  Ag  =  0  (26) 

'  M) 


The  iteration  stops  when  \Ak+r-^k\  <  t,  a  given  small  value 
or  the  number  of  iterations  reaehes  a  pre-speeified 
number.  Then  bringing  the  eonverged  Lagrangian 
multiplier  A  baek  to  Eq.  (19)  or  (23)  provides  the 
eonstrained  optimal  solution. 

Now  eonsider  the  speeial  ease  where  IF  =  L^H,  z  =  Hx , 
and  m  =  0,  that  is,  a  quadratie  eonstraint  on  the  state. 
Under  these  eonditions,  t  =  0  and  e  is  no  longer  a  funetion 
of  A  so  its  derivative  relative  to  A  vanishes,  e  =  0 .  The 
quadratie  eonstrained  solution  is  then  given  by 

3c  =  (W+AM)-'WX  (27a) 


where  the  Lagrangian  multiplier  A  is  obtained  iteratively 
as  in  Eq.  (26)  with  the  eorresponding/^T)  and  f(A)  given 
by 
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7; — r^Tr  +  ™o 
a+Aafy 


/W  =  -2S 


rn+Aafy 


(27b) 

(27c) 


The  solution  of  Eq.  (27)  is  also  called  the  constrained 
least  squares  [Moon  and  Stirling,  2000;  pp  765-766], 
which  was  previously  applied  for  the  joint  estimation  and 
calibration  [Yang  and  Lin,  2004].  When  M  =  0,  the 
constraint  in  Eq.  (13)  degenerates  to  a  linear  one.  The 
constrained  solution  in  Eq.  (19)  is  still  valid.  However,  the 
iterative  solution  for  finding  A  is  no  longer  applicable  and 
a  closed-form  solution  is  available  as  given  in  Eq.  (7). 

4  Simulation  Results 


In  this  section,  a  simple  example  is  used  to  demonstrate 
the  effectiveness  of  the  nonlinear  constrained  method  of 
this  paper  and  to  show  its  superior  performance  as 
compared  to  the  linearized  constrained  method  in  [Simon 
and  Chia,  2002].  In  this  example,  a  ground  vehicle  is 
assumed  to  travel  along  a  circular  road  segment  as  shown 
in  Figure  2  with  the  turn  center  chosen  as  the  origin  of  the 
x-y  coordinates. 


Figure  2  -  Tracking  a  Vehicle  along  a  Circular  Road 


Under  this  assumption,  the  road  constraint  is  quadratic 
and  can  be  written  as 


f{x,y)=x^  +y^  -r^ 


[x  _y] 


“1 

o’ 

X 

0 

1 

y_ 

=  0  (28) 


Assume  that  the  true  position  of  the  vehicle  is  at  x  = 
[rcosO,  rsinO]^  and  an  unbiased  estimate  denoted  by  x  is 
obtained  by  a  certain  means  such  that 

x~N(x,P)  (29) 

where  P  is  the  estimation  error  covariance  matrix,  which 
is  assumed  to  take  a  diagonal  form  as  P  =  diag{[ al , 

a^lj  with  (T^=<T^  =  cr^.  It  is  understandable  that 
additional  errors  will  appear  if  the  estimate  x  is  biased. 


Furthermore,  if  the  error  covariance  matrix  is  not 
diagonal,  the  correlation  direction  will  also  affect  the 
statistical  property.  Ruling  out  such  variability  in 
conditions  will  make  results  analysis  easier  while  not 
losing  the  generality. 

To  apply  the  linear  constrained  Kalman  fdter  of  [Simon 
and  Chia,  2002],  the  nonlinear  constraint  is  linearized 
about  a  previous  constrained  state  denoted  by  x”  and  can 
be  written  as 


(30) 


In  terms  of  the  matrix  notations,  we  have  D  =  [x“  y~] 
and  d  =  r^.  Since  the  previous  constrained  state  x”  is  also 
on  the  circular  path,  it  can  be  parameterized  as  x”  ^ 

[rcos(6+A6),  r  sin(6+A6)]^  ■whcxQ  A6\s  the  angular  offset 
and  its  distance  to  the  true  state  is  given  by 


2r'(l-cosA6>) 


(31) 


With  W  =  F‘  {i.e.,  7/  =  a  and  z  =  Hx ,  where  /„  stands 
for  an  identity  matrix  of  dimension  n),  the  linearized 
constrained  estimate  denoted  by  x^  can  be  calculated 
using  Eq.  (7). 

Similarly,  with  W  =  P'\  M  =  I2,  and  mg  =  r,  the  quadratic 
constrained  estimate  denoted  by  is  given  by  Eq.  (27) 


In  the  first  simulation,  we  set  r=  100  m,  9=  45°,  and  a  = 
10  m  and  then  draw  samples  of  x  from  the  distribution  in 
Eq.  (29)  as  the  unconstrained  state  estimates.  We  then 
calculate  both  the  linearized  and  nonlinear  constrained 
state  estimates  x^  and  x^^  at  two  linearizing  points,  i.e., 

two  previous  constrained  state  estimates  x”,  with  AO^  0° 
and  -15°,  respectively. 

Figures  3  and  4  illustrate  how  the  nonlinear  constrained 
method  of  this  paper  and  the  linearized  constrained 
method  of  [Simon  and  Chia,  2002]  actually  project 
unconstrained  state  estimates  onto  a  nonlinear  constraint, 
which  is  a  circular  road  segment  in  this  example.  In 
Figure  3  for  zl^  =  0°,  the  linearizing  point  (small  circle  o) 
coincides  with  the  tme  state  (star  *)  (the  estimator  is  not 
aware  of,  though).  The  linearized  constraint  is  a  line 
tangent  to  the  circular  path  at  the  point.  There  are  50 
random  samples  drawn  from  the  distribution  of  Eq.  (29) 
as  the  unconstrained  state  estimates  (dot  ■),  which  are 
projected  onto  the  linearized  constraint  using  Eq.  (7)  as 
the  linearized  constrained  estimates  (cross  sign  x)  and 
using  Eq.  (27)  as  the  quadratic  constrained  estimates  (plus 
sign  -I-).  Clearly,  all  the  quadratic  constrained  estimates 
fall  onto  the  circle,  thus  satisfying  the  constraint  whereas 
not  all  linearized  constrained  estimates  do  so. 

In  Figure  4  for  AO  =  -15°,  the  linearizing  point  is  away 
from  the  true  state.  At  100  m,  an  angular  offset  is  related 
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to  a  separation  error  by  a  faetor  of  1.74  m/deg.  This 
eorresponds  to  about  26  m  for  A9  =  -15°.  Although  the 
linearized  method  using  Eq.  (7)  effeetively  projeets  all  the 
uneonstrained  state  estimates  onto  the  linearized 
eonstraint,  the  linearized  eonstraint  itself  is  tilted  off, 
introdueing  larger  errors  due  to  the  eonstraint 
approximation.  In  eontrast,  the  quadratie  eonstrained 
method,  being  independent  of  the  prior  knowledge  about 
the  state  estimate,  easily  satisfies  the  eonstraint. 

In  the  seeond  simulation,  we  draw  5000  uneonstrained 
state  estimates  for  eaeh  linearizing  point  A9,  whieh  varies 
from  -20°  to  20°  in  steps  of  5°.  At  eaeh  linearizing  point, 
we  ealeulate  the  root  mean  squared  (RMS)  errors  between 
the  true  state  and  the  linearized  and  nonlinear  eonstrained 
state  estimates,  respeetively.  Figure  5  shows  the  two 
RMS  values  as  a  funetion  of  the  linearizing  point  together 
with  the  uneonstrained  state  estimation  error  standard 
deviation  c. 


unconstrained  estimates  projected  onto  nonlinear  constraints  at  A6  =  0  deg 


Figure  3  -  Projeetion  of  Uneonstrained  Estimates  onto 
Constraints  ( x”  =  x) 


unconstrained  estimates  projected  onto  nonlinear  constraints  at  A6  =  -15  deg 
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Figure  4  -  Projeetion  of  Uneonstrained  Estimates  onto 
Constraints  ( x”  ^  x) 


As  expeeted,  the  RMS  values  for  the  linearized 
eonstrained  estimates  (eirele  o)  grow  large  as  the  angular 
offset  A9  inereases.  The  RMS  values  for  the  quadratie 
eonstrained  estimates  (dot  ■)  remain  eonstant  and  are 


slightly  smaller  than  the  uneonstrained  estimates,  beeause 
the  seattering  along  a  eonstraint  eurve  is  smaller  than  in  a 
two  dimensional  (2D)  plan.  For  the  same  reason,  the  RMS 
values  for  the  linearized  eonstrained  estimates  are  smaller 
than  those  of  the  quadratie  eonstrained  estimates  for  \A9i  < 
7°.  However,  the  use  of  RMS  values  for  eomparison  is 
less  meaningful  in  this  ease.  As  shown  in  Figure  6,  both 
the  uneonstrained  estimates  (eirele  o)  and  linearized 
eonstrained  estimates  (dot  ■)  do  not  always  satisfy  the 
nonlinear  eonstraints  whereas  quadratie  eonstrained 
estimates  (plus  sign  -I-)  do  all  the  time  (i.e.,  the  eonstraint 
is  satisfied  if  the  distanee  to  the  origin  is  100  m). 


monte  carlo  simulation  5000  runs  per  6 


A6:  angular  offset  from  true  state,  deg 

Figure  5  -  RMS  Values  of  State  Estimates  vs.  Angular 
Offset 


Figure  6  -  Constraint  Satisfaetion 


In  a  2D  setting,  the  eireular  path  eonstraint  of  Eq.  (30) 
allows  for  a  simple  geometry  solution  to  the  problem.  The 
desired  on-eirele  point  x  is  the  interseetion  between  the 
eirele  and  the  line  extending  from  the  origin  to  the  off- 
eirele  point  x .  The  geometry  solution  ean  be  written  as 

X  =  r  cos[tan (^)]  (32a) 

X 

y  =  r  sin[tan  (^)]  (32b) 

X 
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Since  it  is  an  exact  solution  for  this  particular  simulation 
example,  we  can  use  it  to  verify  the  iterative  solution 
obtained  by  the  quadratic  constrained  method.  Figure  7 
shows  the  quadratic  constrained  estimates  (cross  x) 
obtained  by  the  iterative  algorithm  of  Eq.  (27)  and  the 
exact  geometry  solutions  (circle  o),  which  indeed  match 
each  other  perfectly. 

Finally,  we  use  Figure  8  to  show  an  example  of  the 
Lagrangian  multiplier  as  it  is  calculated  iteratively  using 
Eq.  (27).  The  mns  for  five  unconstrained  state  estimates 
are  plotted  in  the  same  figure  and  to  make  it  fit,  the 
normalized  absolute  values  of  A  are  taken.  As  shown, 
starting  from  zero,  it  typically  takes  4  iterations  for  the 
algorithm  to  converge  in  the  example  presented. 

In  fact,  for  this  simple  example  of  a  target  traveling  along 
a  circle  used  in  the  simulation,  a  closed-form  solution  can 
be  derived.  Assume  that  W  ^  I2,  M  =  I2,  0,  and  mo  =  - 

r~ .  The  nonlinear  constraint  can  be  equivalently  written  as: 

/x  =  (33) 

The  quadratic  constrained  estimate  given  in  Eq.  (27a)  is 
repeated  below  for  easy  reference: 

x  =  {W +  XMy'Wx  =  {\  +  Xy^x  (34) 

where  T  is  the  Lagrangian  multiplier. 

Bringing  Eq.  (34)  back  to  Eq.  (33)  gives: 

=  =  (35) 

-  -  l  +  T  l-t-T 


The  solution  for  T  is: 


r  r 


where  ||■||2  stands  for  the  2-norm  or  length  for  the  vector. 

Bringing  the  solution  for  T  in  Eq.  (36)  back  to  Eq.  (34) 
gives: 


This  indicates  that  for  this  particular  case,  the  constraining 
results  in  normalization.  This  is  consistent  with  the 
geometric  solution  shown  in  the  above  simulation. 

This  suggests  a  simple  solution  for  some  practical 
applications.  When  a  target  is  traveling  along  a  circular 
path  (or  approximately  so),  one  can  first  find  the 
equivalent  center  of  the  circle  around  which  to  establish  a 
new  coordinate  system.  Then  express  the  unconstrained 
solution  in  the  new  coordinate  and  normalize  it  as  the 
constrained  solution.  Finally  convert  it  back  to  the 
original  coordinates.  For  non-circular  but  second-order 
paths,  eigenvalue -based  scaling  may  be  effected  following 


coordinate  translation  and  rotation  in  order  to  apply  this 
circular  normalization.  Reverse  operations  are  then  used 
to  transform  back.  For  applications  of  high 
dimensionality,  the  scalar  iterative  solution  of  Eq.  (26) 
may  be  more  efficient. 


unconstrained  estimates  projected  onto  nonlinear  constraints 
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Figure  7  -  Iterative  Quadratic  Constrained  Solution  vs. 
Geometry  Solution 


iterative  solution  of  lagrangian  multiplier:  first  5  data  points 


0.9 

^  ' 

- 

0.8 

■ 

0.7 

- 

- 

1  0.5 

- 

o 

0.4 

- 

0.3 

-  j 

1®*  data  point 

2"^  data  point 

0.2 

—  3”^^  data  point 

• 

4*^  data  point 

0.1 

1 . 

5*^  data  point 

2  4  6  8  10 

12  14 

iteration 


Figure  8  -  Convergence  in  Iterative  Lagrangian  Multiplier 


5  Conclusions 

In  this  paper,  we  have  presented  a  ^3Kod  for 
incorporating  second-order  nonlinear  state  constraints  in  a 
Kalman  filter.  It  is  generalized  from  our  earlier  solutions 
for  quadratic  constraint  functions  to  more  general  second- 
-order  state  constraint  functions.  It  can  be  viewed  as  an 
extension  of  the  Kalman  filter  with  linear  state  equality 
constraints  to  nonlinear  cases.  Simulation  results 
demonstrate  the  performance  of  this  method  as  compared 
to  linearized  state  constraints. 

Although  the  present  solution  considers  a  scalar 
constraint,  it  is  possible  to  extend  the  solution  to  cases 
where  multiple  nonlinear  constraints  or  hybrid  linear  and 
nonlinear  constraints  are  imposed.  Other  directions  for 
future  work  include  search  for  more  efficient  root  finding 
algorithms  to  solve  for  the  Lagrangian  multiplier.  With  a 
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quadratic  cost  function,  it  is  of  interest  to  further  extend 
the  iterative  method  to  explore  other  types  of  nonlinear 
constraints  of  practical  significance. 
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